#ifndef _DISTRIBUTE_OPTIMIZER_H
#define _DISTRIBUTE_OPTIMIZER_H
#include <numeric>
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <iostream>
#include <string>
#include <cmath>
#include <stdlib.h>
#include <std_msgs/Float64.h>
#include <queue>
#include <cstdint>
#include  <algorithm>
#include "formation_avoidance/DistributedOptimize.h"
#include <Eigen/Eigen>
#include "formation_avoidance/AstarPath.h"
#define PARTICLE_NUM 10

const int POW2 = 7;	

class DistributeOptimizer
{
public:
	DistributeOptimizer(int id,int set_agent_num):OptimizerID(id),loop_rate(400),OptimizerColorID(id+32),agent_num(set_agent_num)
	{
		NeighbourVariable.resize(set_agent_num);
		NeighbourGradient.resize(set_agent_num);
		Neighbour_y.resize(set_agent_num);
		variable_gradient_sub.resize(set_agent_num-1);
		selfgetflag.resize(set_agent_num-1);
		neighbourgetflag.resize(set_agent_num-1);
	}
	~DistributeOptimizer(){}

	bool getGradient();
	bool sendVariableAndReceiving();
	bool sendGradient();

	bool receivingVariable();
	bool receivingGradient();

	bool updateVariable();
	void initial_variable(Eigen::VectorXd init_variable);
 	void RegisterCallBack(std::function<double(Eigen::VectorXd,bool,double)> f)
	{
		costJ = f;
	}
	void gradientDescent();
	void init(ros::NodeHandle &nh);
	void optimize_sever_cb(const formation_avoidance::DistributedOptimize::ConstPtr& msg);
	void variable_mapping();
	void updatey();
	void PSO();

	int OptimizerID;
	ros::Rate loop_rate;
	int OptimizerColorID;

    std::function<double(Eigen::VectorXd,bool,double )> costJ;
	Eigen::VectorXd variable;
	Eigen::VectorXd gradient;
	std::vector<Eigen::VectorXd> NeighbourVariable;
	std::vector<Eigen::VectorXd> NeighbourGradient;
	Eigen::VectorXd lastgradient;

	Eigen::VectorXd y;
	std::vector<Eigen::VectorXd> Neighbour_y;

	ros::Publisher variable_gradient_pub;
	std::vector<ros::Subscriber> variable_gradient_sub;
	int optimize_round{0};
	std::vector<int> selfgetflag;
	std::vector<int> neighbourgetflag;
	Eigen::VectorXd lasty;


	int othergetflag{0};
	int megetflag{0};
	bool GDInitFlag{false};

	Eigen::MatrixXd W;

	double current_v;

	Eigen::VectorXd particle[PARTICLE_NUM];
	Eigen::VectorXd particle_v[PARTICLE_NUM];
	std::pair<Eigen::VectorXd,double> localBestParticle[PARTICLE_NUM];
    std::pair<Eigen::VectorXd ,double> globalBestParticle;


	int round;
	int agent_num;

	int optimize_sever_cb_num{0};
};


#endif